//#define DEBUG_CODE

#ifdef DEBUG_CODE
#include <stdlib.h>
#include <stdio.h>
#endif
#include <stdint.h>
#include <math.h>
#include <immintrin.h>

#include "myexp.h"
#include "myexp_impl.h"

#define _mm256_set_m128d(vh, vl) _mm256_insertf128_pd(_mm256_castpd128_pd256(vl), (vh), 1)

#define ALIGN_16 __attribute__((aligned(16)))
#define ALIGN_32 __attribute__((aligned(32)))

static const __m256d MYEXP_MASK_1p45 =  { 0x1.8p45, 0x1.8p45, 0x1.8p45, 0x1.8p45}  ;

// ln(2)= #define LN2	0.69314718055994530942
static const __m256d MYEXP_LN2 = {LN2,LN2,LN2,LN2};

// 1/ln(2)=0x1.71547652b82fe1p+0
static const __m256d MYEXP_INVLN2 =
    {0x1.71547652b82fep+0, 0x1.71547652b82fep+0, 0x1.71547652b82fep+0, 0x1.71547652b82fep+0};

// exp(0x1.6232bdd7abcd24p+9)= 2^1022.000000000000
// MYEXP_N_1022xLN2[i] = 1022 * ln(2) = 0x1.6232bdd7abcd2p+9 = 0x4086232bdd7abcd2
static const uint64_t MYEXP_N1022xLN2[4] ALIGN_32  =
    {LN2_X_1022_AS_UINT64, LN2_X_1022_AS_UINT64, LN2_X_1022_AS_UINT64, LN2_X_1022_AS_UINT64};

static const __m256d MYEXP_C2 = {C2, C2, C2, C2};
static const __m256d MYEXP_C3 = {C3, C3, C3, C3};
static const __m256d MYEXP_C4 = {C4, C4, C4, C4};
static const __m256d MYEXP_C5 = {C5, C5, C5, C5};

static const int  MYEXP_N_1023[4] ALIGN_16 = {1023,0,1023,0};

// consider zi is int32_x8, zi[0],zi[2],zi[4],zi[6] is interger
//static inline __m256d search_table(__m256d zi)
static __m256d search_table(__m256d zi)
{
    __m128i n_1023 = _mm_load_si128((__m128i const *)(MYEXP_N_1023));
    __m128i n_127  = _mm_srli_epi64(n_1023, 3);   

    __m128i m128_idxA = (__m128i)(_mm256_extractf128_pd(zi, 0));
    __m128i m128_idxB = (__m128i)(_mm256_extractf128_pd(zi, 1));

    __m128i top12A = _mm_srli_epi64(m128_idxA, 7);      // signed int64 right shift (>>7) // ki
    __m128i top12B = _mm_srli_epi64(m128_idxB, 7);      // signed int64 right shift (>>7) // ki

    /*
        ki = (zi.i[0] >> 7) ;
        idx = (zi.i[0] & 127);
    */

    m128_idxA = _mm_and_si128(m128_idxA, n_127);
    m128_idxB = _mm_and_si128(m128_idxB, n_127);

    int32_t idx0= _mm_extract_epi32 (m128_idxA, 0);
    int32_t idx1= _mm_extract_epi32 (m128_idxA, 2);
    int32_t idx2= _mm_extract_epi32 (m128_idxB, 0);
    int32_t idx3= _mm_extract_epi32 (m128_idxB, 2);

    int64_t v0= exp_data_52[idx0];
    int64_t v1= exp_data_52[idx1];
    int64_t v2= exp_data_52[idx2];
    int64_t v3= exp_data_52[idx3];

    #define v0_1 m128_idxA
    #define v2_3 m128_idxB

    top12A = _mm_add_epi64 (top12A, n_1023);  // top12A[0]+=1023, top12A[2]+=1023
    top12B = _mm_add_epi64 (top12B, n_1023);  // top12B[0]+=1023, top12B[2]+=1023
    top12A = _mm_slli_epi64 (top12A, 52);   // left shift 52
    top12B = _mm_slli_epi64 (top12B, 52);   // left shift 52

    v0_1= _mm_set_epi64x (v1,v0);
    v2_3= _mm_set_epi64x (v3,v2);
    v0_1= _mm_or_si128 (v0_1, top12A);
    v2_3= _mm_or_si128 (v2_3, top12B);

    __m256d body  = _mm256_set_m128d(_mm_castsi128_pd(v2_3),  _mm_castsi128_pd(v0_1));
    return body;
}


#ifdef DEBUG_CODE
int i32x4_tmp[4] ALIGN_16;

#define DEBUG_MM128(prompt, MM)                \
    _mm_store_si128((__m128i *)i32x4_tmp, MM); \
    printf("%s={", prompt);                    \
    for (int i = 0; i < 4; i++)                \
    {                                          \
        printf("%d,", i32x4_tmp[i]);           \
    }                                          \
    printf("}\n")
#endif

void exp_vec4_avx(double *pr, const double *px)
{
    __m128i low128, hi128;
    __m128i bit_mask;
    __m128i N1022xLN2;  // 1022*ln(2)
    int is_full_zero;

    bit_mask = _mm_cmpeq_epi64(bit_mask, bit_mask);
    bit_mask = _mm_srli_epi64(bit_mask, 1); // For clean bit63, and keep bit0-bit62 don't change

#ifdef DEBUG_CODE
    DEBUG_MM128("bit_mask", bit_mask);
#endif

    low128   = _mm_loadu_si128((__m128i const *)(px));
    hi128    = _mm_loadu_si128((__m128i const *)(px + 2));
    N1022xLN2 = _mm_load_si128((__m128i const *)(MYEXP_N1022xLN2));

    low128 = _mm_and_si128(low128, bit_mask);
    hi128  = _mm_and_si128(hi128, bit_mask);

    low128 = _mm_cmpgt_epi64(low128, N1022xLN2);    //  low128 contain 1s if low128 >= 1022*LN2
    hi128  = _mm_cmpgt_epi64(hi128, N1022xLN2);     //  hi128 contain 1s if low128 >= 1022*LN2
    low128 = _mm_or_si128(low128, hi128);

    is_full_zero = _mm_testz_si128(low128, low128); // is_full_zero means no px[i] >= 1022*LN2
    if (unlikely(!is_full_zero))                    // is not full_zero means some of x[i]>1022*LN2
    {
        _mm256_zeroupper();
        for (int i = 0; i < 4; i++)
            pr[i] = exp_v1(px[i]);
        return ;
    }
    __m256d x      = _mm256_loadu_pd(px);
    __m256d z      = _mm256_mul_pd(x, MYEXP_INVLN2);       // MM_t1= x / ln(2)
    __m256d zi     = _mm256_add_pd(z, MYEXP_MASK_1p45);    // zi[i] low 32bit = (round)(MM_t1 *128)

    /*
        z0是z的近似值，z0具有这样的形式，
        z0 = ki + idx/128.0, -1022< ki <1024, 0 <= idx < 128
        z0 = zi.f - 0x1.8p45;
        r  = x - (z0 * LN2);
    */
    __m256d MM_z0 = _mm256_sub_pd(zi, MYEXP_MASK_1p45);     // z0 = zi - 0x1.8p45;
    MM_z0  = _mm256_mul_pd(MM_z0, MYEXP_LN2);               // z0: z0 * LN2

    __m256d MM_r  = _mm256_sub_pd(x, MM_z0);     // r = x - (z0 * LN2);
    __m256d MM_r2 = _mm256_mul_pd(MM_r, MM_r);   // r2 = r * r;
    __m256d MM_r4 = _mm256_mul_pd(MM_r2, MM_r2); // r4 = (r^2)*(r^2);

    __m256d MM_t1 = _mm256_mul_pd(MM_r,MYEXP_C3); // t1 = r*C3
    __m256d MM_t2 = _mm256_mul_pd(MM_r,MYEXP_C5); // t2 = r*C5

    MM_t1 = _mm256_add_pd(MM_t1, MYEXP_C2);       // t1  = r*C3+C2
    MM_t2 = _mm256_add_pd(MM_t2, MYEXP_C4);       // t2  = r*C5+C4

    MM_t1 = _mm256_mul_pd(MM_t1, MM_r2);          // t1 = (r*C3+C2)*(r^2)
    MM_t2 = _mm256_mul_pd(MM_t2, MM_r4);          // t2 = (r*C5+C4)*(r^4)

    __m256d body  = search_table(zi);

    MM_t1 = _mm256_add_pd(MM_t1, MM_t2);         // t1= (r*C3+C2)*(r^2) + (r*C5+C4)*(r^4) 
    MM_t1 = _mm256_add_pd(MM_t1, MM_r);          // t1= (r*C3+C2)*(r^2) + (r*C5+C4)*(r^4) + r
    
    MM_t1 = _mm256_mul_pd(MM_t1, body);          // t1 = t1*body
    MM_t1 = _mm256_add_pd(MM_t1, body);          // t1 = t1*body + body
    
    _mm256_storeu_pd(pr, MM_t1);
    _mm256_zeroupper();
    return;
}
